#pragma config(Motor,  port1,           frontR,        tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port2,           backRTop,      tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port3,           backRBottom,   tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port4,           frontRTop,     tmotorNormal, openLoop)
#pragma config(Motor,  port5,           backLTop,      tmotorNormal, openLoop)
#pragma config(Motor,  port6,           backLBottom,   tmotorNormal, openLoop)
#pragma config(Motor,  port7,           frontLTop,     tmotorNormal, openLoop)
#pragma config(Motor,  port8,           suckR,         tmotorNormal, openLoop)
#pragma config(Motor,  port9,           suckL,         tmotorNormal, openLoop, reversed)
#pragma config(Motor,  port10,          frontL,        tmotorNormal, openLoop)
//*!!Code automatically generated by 'ROBOTC' configuration wizard               !!*//

#pragma platform(VEX)

//Competition Control and Duration Settings
#pragma competitionControl(Competition)
#pragma autonomousDuration(20)
#pragma userControlDuration(120)

/*INCLUDES*///////////////
#include "Vex_Competition_Includes.c"

/*DEFINITIONS*////////////
#define FULL 127

////////////////////////////////////////////////////////////////////////////
/*FUNCTION LIBRARY*/////////////////////////////////////////////////////////

/*General Funtions*/////

/*Set Drive Left Speed*/
void setDriveLSpeed(int speed) {
  motor[backLTop] = motor[backLBottom] = motor[frontL] = motor[frontLTop] = speed;
}

/*Set Drive Right Speed*/
void setDriveRSpeed(int speed) {
  motor[backRTop] = motor[backRBottom] = motor[frontR] = motor[frontRTop] = speed;
}

/*Set Suck Speed*/
void setSuckSpeed(int speed) {
  motor[suckL] = speed;
  motor[suckR] = speed;
}

/*Set Left Suck Speed*/
void setSuckLSpeed(int speed) {
  motor[suckL] = speed;
}

/*Set Right Suck Speed*/
void setSuckRSpeed(int speed) {
  motor[suckR] = speed;
}

/*Kill Suck Motors*/
void killsuck() {
  setSuckSpeed(0);
}

/*Kill Drive Train Motors*/
void killdrive() {
  setDriveLSpeed(0);
  setDriveRSpeed(0);
}

/*AUTON FUNCTIONS*////////////////////////////////
void drive_forward_msec(int speed, int duration) {
  setDriveLSpeed(speed);
  setDriveRSpeed(speed);
  wait1Msec(duration);
  killdrive();
}

/*AUTON FUNCTIONS*////////////////////////////////
void drive_left_msec(int speed, int duration) {
  setDriveLSpeed(speed);
  wait1Msec(duration);
  killdrive();
}

/*AUTON FUNCTIONS*////////////////////////////////
void drive_right_msec(int speed, int duration) {
  setDriveRSpeed(speed);
  wait1Msec(duration);
  killdrive();
}

/*AUTON FUNCTIONS*////////////////////////////////
void turn_right_msec(int speed, int duration) {
setDriveLSpeed(speed);
setDriveRSpeed(-speed);
  wait1Msec(duration);
  killdrive();
}

/*AUTON FUNCTIONS*////////////////////////////////
void turn_left_msec(int speed, int duration) {
setDriveLSpeed(-speed);
setDriveRSpeed(speed);
  wait1Msec(duration);
  killdrive();
}

void suck_msec(int speed, int duration) {
setSuckLSpeed(speed);
setSuckRSpeed(speed);
wait1Msec(duration);
killsuck();
}

/*END FUNCTION LIBRARY*////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////



///////////////////////////////////////////////////////////////////////////////
/*PRE AUTONOMOUS*//////////////////////////////////////////////////////////////
void pre_auton() {
}

/*AUTONOMOUS*//////////////////////////////////////////////////////////////////
task autonomous() {
drive_forward_msec(50, 300);
suck_msec(-127, 1000);
drive_forward_msec(-127, 310);
//auton skills
//turn_right_msec(127, 200);
wait1Msec(3000);
drive_forward_msec(-127, 500);
drive_forward_msec(127, 300);
wait1Msec(3000);
drive_forward_msec(-127, 500);
drive_forward_msec(127, 300);

//turn_right_msec(127, 500);
//drive_forward_msec(-127, 400);
//suck_msec(127,4000);

}

/*TELE OP*/////////////////////////////////////////////////////////////////////
task usercontrol() {
  while (true) {

  // drive
    setDriveLSpeed(vexRT[Ch3]);
    setDriveRSpeed(vexRT[Ch2]);

  //  suck
    setSuckLSpeed((vexRT[Btn5U] - vexRT[Btn5D])*FULL);
    setSuckRSpeed((vexRT[Btn6U] - vexRT[Btn6D])*FULL);
  }//WHILE
}//TELEOP
//END//////////////////////////////////////////////////////////////////////////
///////////////////////////////////////////////////////////////////////////////
